/* 

对外接口：_Creat(),_Cal()

 */


#include <iostream>

#include "../include/_PI_Cale.h"

#define π 3.141592653589793238462643383279



PI *PI_Creat()
{
    try
    {
        auto p = new PI{
            .kp = 1,
            .ki = 0,
            .upper = 0,
            .lower = 0,
            .y_init = 0,
            .Ts = 1,
            .yi = 0,
            .y = 0};
        return p;
    }
    catch (const std::bad_alloc &e)
    {
        return nullptr;
    }
}

void PI_Cal(
    PI *p,

    float x,
    int update,

    float PI_kp,
    float PI_ki,
    float PI_upper,
    float PI_lower,
    float PI_y_init,
    float Ts,

    float *y)
{    
    auto 输入系数 = [=](PI *p) { 
        //外部变量都隐式按值捕获
        p->ki      = PI_ki;
        p->kp      = PI_kp;
        p->lower   = PI_lower;
        p->upper   = PI_upper;
        p->y_init  = PI_y_init;
        p->Ts      = Ts; 
    };
    输入系数(p);
    
    if(update){
        //非零复位
        p->y = p->y_init;
        p->yi = 0;

    }
    else{
        //不复位
        auto yp = p->kp * x;
        auto yi = p->yi + p->ki * p->Ts * x ;//积分采用后向欧拉
        auto yt = yp +yi;

        //当模块组件之和超出输出限制且积分器输出与模块输入具有相同的符号时，积分停止;
        //当模块组件之和超出输出限制且积分器输出与模块输入具有相反的符号时，积分恢复;
        //钳位有时称为条件积分;
        //对于饱和时间相对较小的被控对象，钳位可能很有用；但对于饱和时间较大的被控对象，它可能产生不良的瞬时响应。
        auto 符号 = ( x>0&&yi>0 )||( x<0&&yi<0 );
        if (yt > p->upper){
            yt = p->upper;
            if(符号) yi = p->yi;
        }
        if (yt < p->lower) {
            yt = p->lower;
            if(符号) yi = p->yi;
        }
        p->yi = yi;
        p->y = yt;
      
    }

    auto 输出 = [=]( float *y ) {
        //外部变量都隐式按值捕获 
        *y = p->y;         
    };
    输出( y );

    

}